import numpy as np
import matplotlib.pyplot as plt
import matplotlib as mpl
import time
import json
import math
import csv
from matplotlib import animation


pi = math.pi

mpl.rcParams['font.sans-serif'] = ['SimHei']  # 指定默认字体
mpl.rcParams['axes.unicode_minus'] = False  # 解决保存图像是负号'-'显示为方块的问题


from psoco import PSOCO,PSOCO_SAMEACC
from ti5_fkik import arm_ik,ti5FK,rotM2Euler_ZYX
from quartic_scurve import QUARTIC_SCURVE



def doubS_uniformAcc(Dmax,V0,B1,B2,C1,C2,D1,D2,X1_,X2_,interp_num,need_only_D):
    """绘制含匀加速的sigmoid曲线

    Args:
       
        
        注： 此曲线为先加速再减速, 输入的B2应大于0, 函数会将B2自动取反
    Returns:
        None: None
        
    按照输入的参数绘制出含匀加速段的非对称式Sigmoid型S曲线
    """
    
    if Dmax <= 0.001:
        return [0]*interp_num, 0
    
    B2 = -B2
    A1 = V0
    A2 = 0
    
    # 计时
    sTime = time.time()
    
    # S曲线参数
    # A1 = 0; B1 = 30; C1 = 12; D1 = 10
    # A2 = 0; B2 = -21; C2 = 12; D2 = 34
    
    # 分段函数
    def S_(x):
            return  A1 + B1/(1+np.exp(-C1*x+D1)) + A2 + B2/(1+np.exp(-C2*x+D2))
    def piece_S(x, X1_, X2_):

        if x <= (D1/C1) :
            return S_(x) 
        elif x <= (D1/C1) + X1_ : 
            return S_(D1/C1) + ((B1*C1)/4)*(x - D1/C1)
        elif x <= (D2/C2) + X1_ : 
            return ((X1_*B1*C1)/4) + S_(x-X1_)
        elif x <= (D2/C2) + X1_ + X2_: 
            return ((X1_*B1*C1)/4) + S_(D2/C2) + (B2*C2/4)*(x - X1_ - D2/C2)
        else :
            return ((X1_*B1*C1)/4)  + (X2_*B2*C2/4) + S_(x- X1_ - X2_)
        pass
    
    def piece_D(x, X1_, X2_):
        
        def D_(x):

            return (A1+B1+A2+B2)*x + (B1/C1)*np.log(1+np.exp(-C1*x+D1)) + (B2/C2)*np.log(1+np.exp(-C2*x+D2)) 
            # return (A1+B1+A2+B2)*x + (B1/C1)*np.log(1+np.exp(-C1*x+D1)) + (B2/C2)*np.log(1+np.exp(-C2*x+D2)) 
            pass
        
        if x <= D1/C1 :
            return D_(x)
        elif x <= D1/C1 + X1_ :
            return D_(D1/C1) \
                    + (S_(D1/C1) + (B1*C1/8)*(x-D1/C1)) * (x-D1/C1)
        elif x <= D2/C2 + X1_ :
            return (S_(D1/C1) + (X1_*B1*C1/8))*X1_ \
                    + D_(x-X1_) \
                    + (X1_*B1*C1/4) * (x-(D1/C1)-X1_)
                    # + (S_(D1/C1)+(X1_*B1*C1/4)) * (x-(D1/C1)-X1_)
        elif x <= D2/C2 + X1_ + X2_:
            return (S_(D1/C1) + (B1*C1/8)*X1_)*X1_ \
                    + D_(D2/C2) \
                    + ((X1_*B1*C1/4) + S_(D2/C2) + (B2*C2/8)*(x-X1_-(D2/C2)))*(x-X1_-(D2/C2)) \
                    + (X1_*B1*C1/4) * ((D2/C2)-(D1/C1))
                    # + (S_(D1/C1)+(X1_*B1*C1/4)) * ((D2/C2)-(D1/C1))
        else :
            return (S_(D1/C1) + (B1*C1/8)*X1_)*X1_ \
                    + ((X1_*B1*C1/4) + S_(D2/C2) + (X2_*B2*C2/8))*X2_ \
                    + D_(x-X1_-X2_) \
                    + (X1_*B1*C1/4) * ((D2/C2)-(D1/C1)) \
                    - (A1+B1+B2)*(x-X1_-X2_-D2/C2) \
                    + (A1+B1+B2+B1*C1*X1_/4+B2*C2*X2_/4)*(x-X1_-X2_-D2/C2)  
                    # + (S_(D1/C1)+(X1_*B1*C1/4)) * ((D2/C2)-(D1/C1)) \
                    # + (piece_S(x,X1_,X2_))*(x-X1_-X2_-D2/C2)  
                    # 梯形1+梯形2+原曲线+矩形-原S曲线最后多余的部分+EV*x
                    
        pass
    
    def piece_A(x,X1_,X2_):
        
        def A_(x):
            return B1*C1*np.exp(-C1*x+D1)/((1+np.exp(-C1*x+D1))**2) \
                    + B2*C2*np.exp(-C2*x+D2)/((1+np.exp(-C2*x+D2))**2)
            pass
        
        if x <= D1/C1:
            return A_(x)
        elif x <= D1/C1 + X1_:
            return A_(D1/C1)
        elif x <= D2/C2 + X1_:
            return A_(x-X1_)
        elif x <= D2/C2 + X1_ + X2_:
            return A_(D2/C2)
        else :
            return A_(x-X1_-X2_)
        pass
    
    def piece_J(x,X1_,X2_):
        
        def J_(x):
            return B1*C1*C1*(1/(1+np.exp(-C1*x+D1)) - 3/((1+np.exp(-C1*x+D1))**2) + 2/((1+np.exp(-C1*x+D1))**3)) \
               + B2*C2*C2*(1/(1+np.exp(-C2*x+D2)) - 3/((1+np.exp(-C2*x+D2))**2) + 2/((1+np.exp(-C2*x+D2))**3))
        
        if x <= D1/C1:
            return J_(x)
        elif x <= D1/C1 + X1_:
            # return J_(D1/C1)
            return 0
        elif x <= D2/C2 + X1_:
            return J_(x-X1_)
        elif x <= D2/C2 + X1_ + X2_:
            # return J_(D2/C2)
            return 0
        else :
            return J_(x-X1_-X2_)
        
        pass
    
    
    # 时间
    tcost = np.abs((D2+D1)/C2) + X1_ + X2_
    
    # 函数
    x = np.linspace(0,tcost,int(interp_num))
    d = np.vectorize(piece_D)(x,X1_,X2_) - (B1*D1/C1 + B2*D2/C2) # D - D(0)
    
    # 若只需要位移插值点，不需要画图
    Ve = V0+B1+B2+(B1*C1*X1_)/4 + (B2*C2*X2_)/4
    Vmax = V0 + B1 + B1*C1*X1_/4
    A1max = B1*C1/4
    A2max = B2*C2/4
    J1max = 0.096225*B1*C1*C1
    J2max = 0.096225*B2*C2*C2
    other_param = (V0,Ve,Vmax,A1max,A2max,J1max,J2max)
    if need_only_D:
        return d,tcost,other_param
    
    s = np.vectorize(piece_S)(x,X1_,X2_)
    # d = Dmax + np.vectorize(piece_D)(x,X1_,X2_)
    
    a = np.gradient(s,x)
    j = np.gradient(a,x)
    A = np.vectorize(piece_A)(x,X1_,X2_)
    J = np.vectorize(piece_J)(x,X1_,X2_)
    
    eTime = time.time()
    print('A1',A1,'B1',B1,'C1',C1,'D1',D1)
    print('A2',A2,'B2',B2,'C2',C2,'D2',D2)
    print('V1max',B1+B1*C1*X1_/4,'A1max',B1*C1/4,'J1max',0.096225*B1*C1*C1)
    print('V2max',B2+B2*C2*X2_/4,'A2max',B2*C2/4,'J2max',0.096225*B2*C2*C2)

    print('V0',V0)
    print('VE',V0+B1+B2+(B1*C1*X1_)/4 + (B2*C2*X2_)/4)
    print('tcost',tcost)
    print('插值点数',len(x))
    print('s time', (sTime)*1000,'ms')
    print('e time', (eTime)*1000,'ms')
    print('comput time', (eTime-sTime)*1000,'ms')
    
    plt.figure()
  
    plt.plot(x,s)
    plt.plot(x,d)

    plt.figure()
    plt.plot(x,A)
    plt.plot(x,J)
    plt.figure()
    
    plt.plot(x,a)
    # plt.plot(x,j)
    plt.show()

    return (d,tcost)
    pass


def DVsync_poly2(s_pose,e_pose,step,trajectory_param):
    
    # 位移同步插值，用于笛卡尔空间
    
    s_x,s_y,s_z = s_pose # 起点坐标
    e_x,e_y,e_z = e_pose # 终点坐标
    
    for dim in range(len(s_pose)):
        D_xyz = np.abs(e_pose[dim]-s_pose[dim]) # 位移
     
    
    
    ps = PSOCO(plotGraph=False)
    
    param_D,\
    param_V0,\
    param_Ve,\
    param_V1max,\
    param_A1max,\
    param_J1max,\
    param_V2max,\
    param_A2max,\
    param_J2max,\
    param_D1 = trajectory_param
    
    
    rDmax,rV0,rB1,rB2,rC1,rC2,rD1,rD2,rX1,rX2=np.vectorize(ps.s_psoco)(D_xyz,
                                              param_V0,
                                              param_Ve,
                                              np.abs(param_V1max-param_V0),
                                              param_A1max,
                                              param_J1max,
                                              np.abs(param_V1max-param_Ve),
                                              param_A2max,
                                              param_J2max,
                                              param_D1)

  
    d_list=[]
    tcost_list=[]
    other_param_list=[]
    for i in range(len(rDmax)):
        
        d ,t_cost,other_param = doubS_uniformAcc(rDmax[i],
                                    rV0[i],rB1[i],rB2[i],rC1[i],rC2[i],rD1[i],rD2[i],rX1[i],rX2[i],
                                    step,
                                    True
                                    )
        d_list.append(d)
        tcost_list.append(t_cost)
        other_param_list.append(other_param)
 
    
    tcost_list = np.array(tcost_list)
    
    # xyz = np.transpose(d_list) 
    xyz = np.transpose(d_list)
    
    for dim in range(3):
        if e_pose[dim]<s_pose[dim]:
            xyz[:,dim] = -xyz[:,dim]
            
            
    xyz = xyz + np.tile(np.reshape(s_pose,(1,3)),(step,1))
    # 逆解之前将单位转换为mm
    xyz = xyz*1000 
    
    EulerZYX = np.tile(np.reshape(np.array([90*pi/180,0,180*pi/180]),(1,3)), (step,1))

    xyz_EulerZYX = np.concatenate((xyz,EulerZYX), axis=1)
    
    print(type(xyz_EulerZYX),xyz_EulerZYX.shape,tcost_list)
    print(xyz_EulerZYX[0:10])
    inverse_Jdeg = arm_ik(xyz_EulerZYX)
    
    print(inverse_Jdeg.shape)
    print(inverse_Jdeg[0:10])
    
    return inverse_Jdeg
    
    pass

def DVsync_poly(s_pose,e_pose,step,trajectory_param, res_tcos=0):
    "笛卡尔空间规划，输入：起点，终点，插补步数，轨迹参数 单位/m(Dmax,V0,Ve,V1max,A1max,J1amx,V2amx,A2max,J2max,D1)"
    
    
    # 位移同步插值，用于笛卡尔空间
    
    s_x,s_y,s_z = s_pose # 起点坐标
    e_x,e_y,e_z = e_pose # 终点坐标
    
    
    D_xyz = np.abs(e_pose-s_pose) # 位移
     
    
    
    ps = PSOCO(plotGraph=False)
    
    param_D,\
    param_V0,\
    param_Ve,\
    param_V1max,\
    param_A1max,\
    param_J1max,\
    param_V2max,\
    param_A2max,\
    param_J2max,\
    param_D1 = trajectory_param
    
    param_D = D_xyz
    
    zero_Dindex = []
    for dim in range(len(s_pose)):
        if param_D[dim] <= 0.000001:
            zero_Dindex.append(dim) # 位移为0的执行器序号
    
    Param_Met = np.ones((len(s_pose),6))*np.inf # 参数比矩阵
    for dim in range(len(s_pose)):
        if param_D[dim] > 0.000001:
            Param_Met[dim,0] = param_V1max[dim]/param_D[dim]
            Param_Met[dim,1] = param_A1max[dim]/param_D[dim]
            Param_Met[dim,2] = param_J1max[dim]/param_D[dim]
            Param_Met[dim,3] = param_V2max[dim]/param_D[dim]
            Param_Met[dim,4] = param_A2max[dim]/param_D[dim]
            Param_Met[dim,5] = param_J2max[dim]/param_D[dim]
    
    min_param = np.min(Param_Met) # 获取参数比矩阵最小值
    min_index = np.argmin(Param_Met)
    
    # 获取最小值所在的执行器序号
    row_index, col_index = np.unravel_index(min_index, Param_Met.shape)
    
    # 以最小值所在执行器为基础规划
    rDmax,rV0,rB1,rB2,rC1,rC2,rD1,rD2,rX1,rX2=ps.s_psoco(D_xyz[row_index],
                                              param_V0[row_index],
                                              param_Ve[row_index],
                                              param_V1max[row_index],
                                              param_A1max[row_index],
                                              param_J1max[row_index],
                                              param_V1max[row_index],
                                              param_A2max[row_index],
                                              param_J2max[row_index],
                                              param_D1[row_index])
    print('rDmax,rV0,rB1,rB2,rC1,rC2,rD1,rD2,rX1,rX2')
    print(rDmax,rV0,rB1,rB2,rC1,rC2,rD1,rD2,rX1,rX2)
    ds_coef = param_D/param_D[row_index] # 同步比率

    
    # 获取基础路径V0,VE,Vmax,A1max,A2max,J1max,J2max
    d ,t_cost,other_param = doubS_uniformAcc(rDmax,
                                rV0,rB1,rB2,rC1,rC2,rD1,rD2,rX1,rX2,
                                step,
                                True
                                )
    print('t_cost',t_cost)
    print('V0,VE,Vmax,A1max,A2max,J1max,J2max')
    print(other_param)
    d_list=[]
    tcost_list=[]
    for dim in range(len(s_pose)): # 以基础轨迹 按位移速度同步
        sync_d = ds_coef[dim] * d 
        d_list.append(sync_d)
        tcost_list.append(t_cost)
        pass
    
    tcost_list = np.array(tcost_list)
    xyz = np.transpose(d_list)
    
    for dim in range(len(s_pose)):
        if e_pose[dim]<s_pose[dim]:
            xyz[:,dim] = -xyz[:,dim]
            
    # 加上起点位置
    xyz = xyz + np.tile(np.reshape(s_pose,(1,len(s_pose))),(step,1))
    
    # 逆解之前将单位转换为mm
    xyz = xyz*1000 
    
    EulerZYX = np.tile(np.reshape(np.array([90*pi/180,0,180*pi/180]),(1,3)), (step,1))

    xyz_EulerZYX = np.concatenate((xyz,EulerZYX), axis=1)
    
    # print(type(xyz_EulerZYX),xyz_EulerZYX.shape,tcost_list)
    # print(xyz_EulerZYX[0:10])
    inverse_Jdeg = arm_ik(xyz_EulerZYX)
    
    # print(inverse_Jdeg.shape)
    # print(inverse_Jdeg[0:10])
    if res_tcos:
        return inverse_Jdeg,xyz_EulerZYX,t_cost
    
    return inverse_Jdeg,xyz_EulerZYX
    
    pass

def timesync_poly(s_pose,e_pose,poly_step,trajectory_param):
    """时间同步轨迹规划

    Args:
        s_pose (_type_): _description_
        e_pose (_type_): _description_
        poly_step (_type_): _description_
        trajectory_param (_type_):Param_V0,Param_Ve,Param_Vmax,
                                    Param_A1max,Param_J1max,
                                    Param_A2max,Param_J2max,Param_D1

    Returns:
        _type_: _description_
    """
    # 时间同步插值，用于关节空间
    dim = len(s_pose) # 执行器个数

    Param_D =np.abs(e_pose - s_pose) # 位移绝对值
    
    Param_V0, \
    Param_Ve, \
    Param_Vmax, \
    Param_A1max, \
    Param_J1max, \
    Param_A2max, \
    Param_J2max, \
    Param_D1 = trajectory_param # 除位移外的运动学约束
    
    # 输入度数单位，规划时使用弧度单位
    Param_D = Param_D*pi/180 
    Param_V0 = Param_V0*pi/180
    Param_Ve = Param_Ve*pi/180
    Param_Vmax = Param_Vmax*pi/180
    Param_A1max = Param_A1max*pi/180
    Param_J1max = Param_J1max*pi/180
    Param_A2max = Param_A2max*pi/180
    Param_J2max = Param_J2max*pi/180
    
    
    
    Zero_D_index = [] # 位移为0的执行器的序号
    
    for i in range(dim):
        if Param_D[i] <=0.0001747: # 小于0.01°的位移按0计算
            Zero_D_index.append(i)
            pass
        
    # 保证Zero_D_index内的元素为 0位移执行器个数+1
    Zero_D_index.append(np.inf)
      
    # 除位移为0的执行器外，计算所有执行器轨迹参数
    ps = PSOCO(plotGraph=False)
    rDmax=[];rV0=[];rB1=[];rB2=[];rC1=[];rC2=[];rD1=[];rD2=[];rX1=[];rX2=[] # 参数列表
    d_list = np.zeros((poly_step,dim)) # 规划的轨迹
    tcost_list=[] # 每条轨迹对应的执行时间
    other_param_list=[] # V0,VE,Vmax,A1max,A2max,J1max,J2max
    j = 0
    for i in range(dim):
        if i != Zero_D_index[j]:
            # 轨迹参数
            Dmax,V0,B1,B2,C1,C2,D1,D2,X1,X2 = ps.s_psoco(Param_D[i],
                                                        Param_V0[i],
                                                        Param_Ve[i],
                                                        Param_Vmax[i],
                                                        Param_A1max[i],
                                                        Param_J1max[i],
                                                        Param_Vmax[i],
                                                        Param_A2max[i],
                                                        Param_J2max[i],
                                                        Param_D1[i])
            # rDmax.append(Dmax);rV0.append(V0);rB1.append(B1)
            # rB2.append(B2);rC1.append(C2);rC2.append(C2)
            # rD1.append(D1);rD2.append(D2);rX1.append(X1);rX2.append(X2)
            
            rDmax.append(Dmax)
            
            # 轨迹及执行时间
            d ,t_cost,other_param = doubS_uniformAcc(Dmax,
                                        V0,B1,B2,C1,C2,D1,D2,X1,X2,
                                        poly_step,
                                        True
                                        )
            d_list[:,i] = d
            tcost_list.append(t_cost)
            other_param_list.append(other_param)
        else:
            d_list[:,i] = np.ravel(np.zeros((poly_step,1)))
            rDmax.append(0)
            tcost_list.append(0)
            other_param_list.append((0,0,0,0,0,0,0))
            j += 1
        pass
    
    # 无需转换为 shape(poly_step,dim) 因为定义时即是d_list = np.zeros((poly_step,dim))
    # d_list = np.transpose(np.array(d_list))
    
    # 计算误差
    err_D = Param_D - rDmax # 位移误差
    if np.any(err_D > 0.2):  # 位移误差大于0.2°则认为规划失败
        print('err are more than 0.2')
        return 
    
    for i in range(dim):
        if e_pose[i] < s_pose[i]:
            d_list[:,i] = -d_list[:,i]
    
    for i in range(dim):
        print('tcost',tcost_list[i])
        print('other_param','V0,VE,Vmax,A1max,A2max,J1max,J2max')
        print(other_param_list[i])
    
    # 按最长时间同步
    max_tcost = np.max(tcost_list)
    tcost_list = np.ones((dim,1))*max_tcost
    tcost_list = np.ravel(tcost_list)
    
    # 输出前转换为弧度单位，加上起点
    d_list = d_list*180/pi + np.tile(np.reshape(s_pose,(1,dim)),(poly_step,1)) 
    
    
    print('sync_tcost',tcost_list[0])
    
    return d_list,tcost_list
    
    pass

def timesync_poly_sameAcc(s_pose,e_pose,poly_step,trajectory_param):
    """时间同步轨迹规划

    Args:
        s_pose (_type_): _description_
        e_pose (_type_): _description_
        poly_step (_type_): _description_
        trajectory_param (_type_):Param_V0,Param_Ve,Param_Vmax,
                                    Param_A1max,Param_J1max,
                                    Param_A2max,Param_J2max,Param_D1

    Returns:
        _type_: _description_
    """
    # 时间同步插值，用于关节空间
    dim = len(s_pose) # 执行器个数

    Param_D =np.abs(e_pose - s_pose) # 位移绝对值
    
    Param_V0, \
    Param_Ve, \
    Param_Vmax, \
    Param_A1max, \
    Param_J1max, \
    Param_A2max, \
    Param_J2max, \
    Param_D1 = trajectory_param # 除位移外的运动学约束
    
    # 输入度数单位，规划时使用弧度单位
    Param_D = Param_D*pi/180 
    Param_V0 = Param_V0*pi/180
    Param_Ve = Param_Ve*pi/180
    Param_Vmax = Param_Vmax*pi/180
    Param_A1max = Param_A1max*pi/180
    Param_J1max = Param_J1max*pi/180
    Param_A2max = Param_A2max*pi/180
    Param_J2max = Param_J2max*pi/180
    
    Zero_D_index = [] # 位移为0的执行器的序号
    
    for i in range(dim):
        if Param_D[i] <=0.0001747: # 小于0.01°的位移按0计算
            Zero_D_index.append(i)
            pass
        
    # 保证Zero_D_index内的元素为 0位移执行器个数+1
    Zero_D_index.append(np.inf)
      
    # 除位移为0的执行器外，计算所有执行器轨迹参数
    ps = PSOCO_SAMEACC(plotGraph=False)
    
    rDmax=[];rV0=[];rB1=[];rB2=[];rC1=[];rC2=[];rD1=[];rD2=[];rX1=[];rX2=[] # 参数列表
    d_list = np.zeros((poly_step,dim)) # 规划的轨迹
    tcost_list=[] # 每条轨迹对应的执行时间
    other_param_list=[] # V0,VE,Vmax,A1max,A2max,J1max,J2max
    j = 0
    for i in range(dim):
        if i != Zero_D_index[j]:
            # 轨迹参数
            Dmax,V0,B1,B2,C1,C2,D1,D2,X1,X2 = ps.s_psoco(Param_D[i],
                                                        Param_V0[i],
                                                        Param_Ve[i],
                                                        Param_Vmax[i],
                                                        Param_A1max[i],
                                                        Param_J1max[i],
                                                        Param_Vmax[i],
                                                        Param_A2max[i],
                                                        Param_J2max[i],
                                                        Param_D1[i])
            # rDmax.append(Dmax);rV0.append(V0);rB1.append(B1)
            # rB2.append(B2);rC1.append(C2);rC2.append(C2)
            # rD1.append(D1);rD2.append(D2);rX1.append(X1);rX2.append(X2)
            
            rDmax.append(Dmax)
            
            # 轨迹及执行时间
            d ,t_cost,other_param = doubS_uniformAcc(Dmax,
                                        V0,B1,B2,C1,C2,D1,D2,X1,X2,
                                        poly_step,
                                        True
                                        )
            d_list[:,i] = d
            tcost_list.append(t_cost)
            other_param_list.append(other_param)
        else:
            d_list[:,i] = np.ravel(np.zeros((poly_step,1)))
            rDmax.append(0)
            tcost_list.append(0)
            other_param_list.append((0,0,0,0,0,0,0))
            j += 1
        pass
    
    # 无需转换为 shape(poly_step,dim) 因为定义时即是d_list = np.zeros((poly_step,dim))
    # d_list = np.transpose(np.array(d_list))
    
    # 计算误差
    err_D = Param_D - rDmax # 位移误差
    if np.any(err_D > 0.2):  # 位移误差大于0.2°则认为规划失败
        print('err are more than 0.2')
        return 
    
    for i in range(dim):
        if e_pose[i] < s_pose[i]:
            d_list[:,i] = -d_list[:,i]
    
    for i in range(dim):
        print('tcost',tcost_list[i])
        print('other_param','V0,VE,Vmax,A1max,A2max,J1max,J2max')
        print(other_param_list[i])
    
    # 按最长时间同步
    max_tcost = np.max(tcost_list)
    tcost_list = np.ones((dim,1))*max_tcost
    tcost_list = np.ravel(tcost_list)
    
    # 输出前转换为弧度单位，加上起点
    d_list = d_list*180/pi + np.tile(np.reshape(s_pose,(1,dim)),(poly_step,1)) 
    
    
    print('sync_tcost',tcost_list[0])
    
    return d_list,tcost_list
    
    pass

def save_Jdeg2txt(inverse_Jdeg,dir, use_defualt_head_msg=1,head={'name':'save_Jdeg2txt'}):
    
    # 将机械臂关节角保存在txt文件里
    
    groups_jdeg, aixs = inverse_Jdeg.shape
    if aixs!=6:
        print('err, aixs is not 6')
        return
    
    with open(dir, mode='w') as f:
        if use_defualt_head_msg:
            head = {'name':'Jdeg', 'writeTime':time.time(), 
                    'seq':123456,'shape':[groups_jdeg,6]}
        
            
        msg_head = json.dumps(head)
        f.write(msg_head)
    
        for i in range(0, groups_jdeg):
            w_msg = "\n{}, {}, {}, {}, {}, {}".format(inverse_Jdeg[i,0],
                                                        inverse_Jdeg[i,1],
                                                        inverse_Jdeg[i,2],
                                                        inverse_Jdeg[i,3],
                                                        inverse_Jdeg[i,4],
                                                        inverse_Jdeg[i,5]
                                                        )
            f.write(w_msg)
            pass
    print('inverse_Jdeg write to {}'.format(dir))

    
    
    pass

def save_xyzEuleZYX2txt(mode,param,dir = 'C:\\WorkFile\\MatlabWork\\Ti5\\matlabsim\\xyzZYX_from_Spoly.txt',use_defualt_head_msg=1,head={'name':'save_Jdeg2txt'}):
    """将xyz位置和ZYX欧拉角保存在文档中

    Args:
        mode (_type_): 'Jdeg'，输入6轴角度，保存xyzEuleZYX；
                       'xyz'，三维位置，保存xyz； 暂未实现
                       'xyzEulerZYX'，输入位置和欧拉角，保存xyzEuleZYX
        param (_type_): 输入的参数
        dir (_type_): _description_. 
    """
    if mode=='Jdeg':
        inverse_Jdeg = param
        groups_jdeg, aixs = inverse_Jdeg.shape
        if aixs!=6:
            print('err, aixs is not 6')
            return
        xyzEuler_ZYX = np.zeros((groups_jdeg,6))
        for i in range(groups_jdeg):
            tfM = ti5FK(inverse_Jdeg[i,:]*pi/180)
            xyz = tfM[0:3,3]
            Euler_ZYX =np.array(rotM2Euler_ZYX(tfM[0:3,0:3])) 
            xyzEuler_ZYX[i,:] = np.concatenate((np.reshape(xyz,(1,3)),np.reshape(Euler_ZYX,(1,3))),axis=1)
            
        with open(dir, mode='w') as f:
            if use_defualt_head_msg:
                head = {'name':'Jdeg', 'writeTime':time.time(), 
                        'seq':'xyzEuler_ZYX','shape':[groups_jdeg,6]}
            msg_head = json.dumps(head)
            f.write(msg_head)
        
            for i in range(0, groups_jdeg):
                w_msg = "\n{}, {}, {}, {}, {}, {}".format(xyzEuler_ZYX[i,0],
                                                            xyzEuler_ZYX[i,1],
                                                            xyzEuler_ZYX[i,2],
                                                            xyzEuler_ZYX[i,3],
                                                            xyzEuler_ZYX[i,4],
                                                            xyzEuler_ZYX[i,5]
                                                            )
                f.write(w_msg)
                pass
        print('xyzEuler_ZYX write to {}'.format(dir))
        pass
    elif mode=='xyz':
        
        
        pass
    elif mode=='xyzEulerZYX':
        xyzEuler_ZYX = param
        groups, aixs = xyzEuler_ZYX.shape
        if aixs!=6:
            print('err, aixs is not 6')
            return
        with open(dir, mode='w') as f:
            if use_defualt_head_msg:
                head = {'name':'Pose', 'writeTime':time.time(), 
                        'seq':'xyzEuler_ZYX','shape':[groups,6]}
            msg_head = json.dumps(head)
            f.write(msg_head)
        
            for i in range(0, groups):
                w_msg = "\n{}, {}, {}, {}, {}, {}".format(xyzEuler_ZYX[i,0],
                                                            xyzEuler_ZYX[i,1],
                                                            xyzEuler_ZYX[i,2],
                                                            xyzEuler_ZYX[i,3],
                                                            xyzEuler_ZYX[i,4],
                                                            xyzEuler_ZYX[i,5]
                                                            )
                f.write(w_msg)
                pass
        print('xyzEuler_ZYX write to {}'.format(dir))
        pass
    else:
        print('mode err')
        return    
    
    
    
    
    pass


def JointSpace_poly(dir,s_Jdeg,e_Jdeg,poly_step,V0_list,Ve_list,Vmax_list,A1max_list,J1max_list,A2max_lsit,J2max_list,D1_list):
    
    # 关节空间规划
    
    d_list, tcost_list = timesync_poly(s_Jdeg,e_Jdeg,poly_step,(V0_list,
                                                                Ve_list,
                                                                Vmax_list,
                                                                A1max_list,
                                                                J1max_list,
                                                                A2max_lsit,
                                                                J2max_list,
                                                                D1_list))
    
    save_Jdeg2txt(d_list,dir)
    return tcost_list
    pass

def CartesianSpace_poly():
    
    # 笛卡尔空间规划
    
    
    
    pass


def getData_from_txt(dir = 'C:\\WorkFile\\MatlabWork\\Ti5\\matlabsim\\Jdeg_from_Spoly.txt'):
    "读取txt的数据, 返回Jdeg,eefXYZ"
    
    
    try:
        with open(dir, mode='r') as Jfile:
            Jdata = Jfile.readlines()
            pass
    except Exception as e:
        print(e)
        print('open file hand_eyeJ failed!')
        return
    
    groups_jdeg = (len(Jdata)-1)
    Jdeg = np.zeros((groups_jdeg, 6), dtype=np.float) # type:np.ndarray
    h_eQ = np.zeros((groups_jdeg, 4), dtype=np.float) # type:np.ndarray
    eefXYZ = np.zeros((groups_jdeg, 3), dtype=np.float) # type:np.ndarray
    
    print("open file!")
    print("groups of Jdeg:",groups_jdeg)
    for i in range(0,groups_jdeg):
        Jdata_split = Jdata[1+i].split(',')
        # print(Jdata_split)
        Jdeg[i,0] = float(Jdata_split[0])
        Jdeg[i,1] = float(Jdata_split[1])
        Jdeg[i,2] = float(Jdata_split[2])
        Jdeg[i,3] = float(Jdata_split[3])
        Jdeg[i,4] = float(Jdata_split[4])
        Jdeg[i,5] = float(Jdata_split[5])
    
        tF = ti5FK(Jdeg[i]*pi/180)
        eefXYZ[i,0] = tF[0,3]
        eefXYZ[i,1] = tF[1,3]
        eefXYZ[i,2] = tF[2,3]
    
    return Jdeg,eefXYZ
    
    pass

def get_xyzEulerZYX(mode,dir = 'C:\\WorkFile\\MatlabWork\\Ti5\\matlabsim\\xyzZYX_from_Spoly.txt'):
    "从txt文档读取位姿数据，返回xyzEulerZYX"
    
    try:
        with open(dir, mode='r') as file:
            data = file.readlines()
            pass
    except Exception as e:
        print(e)
        print('open file failed!')
        return
    
    groups_data = (len(data)-1)
    Jdeg = np.zeros((groups_data, 6), dtype=np.float) # type:np.ndarray
    h_eQ = np.zeros((groups_data, 4), dtype=np.float) # type:np.ndarray
    eefpose = np.zeros((groups_data, 6), dtype=np.float) # type:np.ndarray
    
    print("open file!")
    print("groups of Jdeg:",groups_data)
    if mode=='jdeg2xyz':
        for i in range(0,groups_data):
            data_split = data[1+i].split(',')
            # print(Jdata_split)
            eefpose[i,0] = float(data_split[0])
            eefpose[i,1] = float(data_split[1])
            eefpose[i,2] = float(data_split[2])
            eefpose[i,3] = float(data_split[3])
            eefpose[i,4] = float(data_split[4])
            eefpose[i,5] = float(data_split[5])
    elif mode=='xyzEulerZYX':
        for i in range(0,groups_data):
            data_split = data[1+i].split(',')
            # print(Jdata_split)
            eefpose[i,0] = float(data_split[0])
            eefpose[i,1] = float(data_split[1])
            eefpose[i,2] = float(data_split[2])
            eefpose[i,3] = float(data_split[3])
            eefpose[i,4] = float(data_split[4])
            eefpose[i,5] = float(data_split[5])
    else:
        print("mode err!!!")
        return
    
    return eefpose
    
    pass


def plot_from_data(mode='Jdeg',indexToPlot=[0],t=[1.0],data=None, defualt_xticks=0):
    
    if mode=='Jdeg':
        if data.any()!=None:
            Jdeg=data
        else:
            Jdeg, _ = getData_from_txt()
            
        row,cul = Jdeg.shape
     
        plot_point = int(row) # 要画出的插值点数
        
        # row = 980 # 修改要显示的插值点数
        # Jdeg = Jdeg[10:row+10,:]
        
        
        plot_num = len(indexToPlot)
        
        x = np.linspace(0,t,row)
        y = np.zeros((row,plot_num))
        dy = np.zeros((row,plot_num))
        ddy = np.zeros((row,plot_num))
        dddy = np.zeros((row,plot_num))
        for i in range(plot_num):
            y[:,i] = Jdeg[:,indexToPlot[i]]
            dy[:,i] = np.gradient(y[:,i],x)
            ddy[:,i] = np.gradient(dy[:,i],x)
            dddy[:,i] = np.gradient(ddy[:,i],x)
        # dddy[0:10,:] = 0
        # dddy[990:1000,:] = 0
        

        linestyle_list=['-','-','--',':',':','--']
        plt.figure()
        plt.rcParams.update({'font.size': 9})     #设置图例字体
        # 子图1用于显示图例***********************************************
        plt.subplot(5,1,1) 
        ax = plt.gca() # get current axis
        ax.spines['right'].set_color('none')  # spines 是图的四周边框
        ax.spines['top'].set_color('none')
        ax.spines['left'].set_color('none')
        ax.spines['bottom'].set_color('none')
        # plt.axes().get_xaxis().set_visible(False) # 隐藏x坐标轴
        # plt.axes().get_yaxis().set_visible(False) # 隐藏y坐标轴
        plt.yticks([100],
                [''] # 使用正则表达式加转译 （latex公式）                   
                ) 
        plt.xticks([100],
                [''] # 使用正则表达式加转译 （latex公式）                   
                ) 
        plt.ylim((300,301))
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],label='轴{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])
            plt.legend( ncol=3, # 图例显示几列，默认为1
                    loc='lower center' # 图例位置(默认best 为自动调整图例位置)
                    ) # 显示自定义格式图例
        
        

        plt.subplot(5,1,2)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('位移')
        plt.ylabel(r'$d/\rm rad$')
        if not defualt_xticks:
            plt.yticks([-1,0,1,2],
            ['-1.0','0','1.0','2.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
            ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],label='轴{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])
            # plt.legend( ncol=3, # 图例显示几列，默认为1
            #         loc='best' # 图例位置(默认best 为自动调整图例位置)
            #         ) # 显示自定义格式图例
            
        # plt.figure()
        plt.subplot(5,1,3)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('速度')
        plt.ylabel(r'$v/\rm (rad/s)$')
        if not defualt_xticks:
            plt.yticks([-0.5,0,0.5,1.0,1.45],
            ['-0.5','0','0.5','1.0','1.5'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
            ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],dy[0:plot_point,i],linestyle=linestyle_list[i])
            
        # plt.figure()
        plt.subplot(5,1,4)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加速度')
        # plt.ylabel(r'$a/(rad{\cdot}s^{-2})$')
        plt.ylabel(r'$a/\rm (rad/s^{2})$')
        if not defualt_xticks:
            plt.yticks([-5.0,-2.5,0,2.5,5.0],
            ['-5.0','-2.5','0','2.5','5.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
            ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],ddy[0:plot_point,i],linestyle=linestyle_list[i])
            
        # plt.figure()
        plt.subplot(5,1,5)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加加速度')
        plt.ylabel(r'$j/\rm (rad/s^{3})$')
        plt.xlabel('时间/{}'.format(r'$s$'))
        if not defualt_xticks:
            plt.yticks([-20,0,20],
            ['-20.0','0','20.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
            ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],dddy[0:plot_point,i],linestyle=linestyle_list[i])
      
        print('tcost',t)
        
        plt.show()
        pass
    elif mode=='pose':
        if  type(data)==type(None):
            Jdeg = get_xyzEulerZYX()
        elif data.any()!=None:
            Jdeg=data
        else:
            pass
            
        row,cul = Jdeg.shape
        
        plot_point = int(row) # 要画出的插值点数
        plot_num = len(indexToPlot)
        
        x = np.linspace(0,t,row)
        y = np.zeros((row,plot_num))
        dy = np.zeros((row,plot_num))
        ddy = np.zeros((row,plot_num))
        dddy = np.zeros((row,plot_num))
        for i in range(plot_num):
            y[:,i] = Jdeg[:,indexToPlot[i]]
            dy[:,i] = np.gradient(y[:,i],x)
            ddy[:,i] = np.gradient(dy[:,i],x)
            dddy[:,i] = np.gradient(ddy[:,i],x)
        # dddy[0:10,:] = 0
        # dddy[990:1000,:] = 0
    
        linestyle_list=['-','-','--',':',':','--'] 
        plt.figure()
        plt.rcParams.update({'font.size': 9})     #设置图例字体
        # 子图1用于显示图例****************************************
        plt.subplot(5,1,1) 
        ax = plt.gca() # get current axis
        ax.spines['right'].set_color('none')  # spines 是图的四周边框
        ax.spines['top'].set_color('none')
        ax.spines['left'].set_color('none')
        ax.spines['bottom'].set_color('none')
        # plt.axes().get_xaxis().set_visible(False) # 隐藏x坐标轴
        # plt.axes().get_yaxis().set_visible(False) # 隐藏y坐标轴
        if not defualt_xticks:
            plt.yticks([100],
                    [''] # 使用正则表达式加转译 （latex公式）                   
                    ) 
            plt.xticks([100],
                    [''] # 使用正则表达式加转译 （latex公式）                   
                    ) 
            plt.ylim((300,301))
        label_font=['x','y','z']
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],label='{}轴'.format(label_font[i]),linestyle=linestyle_list[i])
            plt.legend( ncol=3, # 图例显示几列，默认为1
                    loc='lower center' # 图例位置(默认best 为自动调整图例位置)
                    ) # 显示自定义格式图例
      
        #位移*************************************************************
        plt.subplot(5,1,2)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('位移')
        plt.ylabel(r'$d/\rm mm$')
        if not defualt_xticks:
            # plt.yticks([0,25,50],
            # ['0','25','50'] # 使用正则表达式加转译 （latex公式）                   
            # ) 
            plt.xticks([0,0.15,0.3,0.45,0.597],
            ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],linestyle=linestyle_list[i])
            # plt.legend( ncol=3, # 图例显示几列，默认为1
            #         loc='best' # 图例位置(默认best 为自动调整图例位置)
            #         ) # 显示自定义格式图例
        
        #速度***************************************************************
        plt.subplot(5,1,3)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('速度')
        plt.ylabel(r'$v/\rm (mm/s)$')
        # plt.xlim((-1,2))
        if not defualt_xticks:
            plt.ylim((-5,55)) 
            plt.yticks([0,25,50],
            ['0','25','50'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.15,0.3,0.45,0.597],
            ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],dy[0:plot_point,i],linestyle=linestyle_list[i])
        
        #加速度***************************************************************  
        plt.subplot(5,1,4)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加速度')
        # plt.ylabel(r'$a/(mm{\cdot}s^{-2})$')
        plt.ylabel(r'$a/\rm (mm/s^{2})$')
        if not defualt_xticks:
            plt.ylim((-450,450))
            plt.yticks([-400,-200,0,200,400],
            ['-400','-200','0','200','400'] # 使用正则表达式加转译 （latex公式）                   
            ) 
            plt.xticks([0,0.15,0.3,0.45,0.597],
            ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],ddy[0:plot_point,i],linestyle=linestyle_list[i])
        
        #加加速度**************************************************************
        plt.subplot(5,1,5)
        ax = plt.gca() # get current axis
        ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加加速度')
        plt.ylabel(r'$j/\rm (mm/s^{3})$')
        plt.xlabel('时间/{}'.format(r'$s$'))
        if not defualt_xticks:
            # plt.yticks([-20,0,20],
            # ['-20.0','0','20.0'] # 使用正则表达式加转译 （latex公式）                   
            # ) 
            plt.xticks([0,0.15,0.3,0.45,0.597],
            ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
            ) 
        for i in range(plot_num):
            plt.plot(x[0:plot_point],dddy[0:plot_point,i],linestyle=linestyle_list[i])
      
        print('tcost',t)
        
        plt.show()
        pass
    pass


def time_poly():
    
    dir = 'C:\WorkFile\MatlabWork\Ti5\matlabsim\Jdeg_from_Spoly.txt'
    s_Jdeg = np.array([0,-30,0,-60,0,0])
    e_Jdeg = np.array([120,30,45,60,-45,30]) 
    poly_step = 1000
    V0_list = np.array([0,0,0,0,0,0])
    Ve_list = np.array([0,0,0,0,0,0])
    Vmax_list = np.array([458.37,572.96,572.96,286.48,286.48,286.48])
    A1max_list = np.array([572.96,687.55,687.55,458.37,458.37,458.37])
    J1max_list = np.array([1718.87,2291.83,2291.83,1145.92,1145.92,1145.92])
    A2max_lsit = np.array([572.96,687.55,687.55,458.37,458.37,458.37])
    J2max_list = np.array([1718.87,2291.83,2291.83,1145.92,1145.92,1145.92])
    D1_list = np.array([10,10,10,10,10,10])
    # D1_list = np.array([7,7,7,7,7,7])
    
    t_list=JointSpace_poly(dir,
                            s_Jdeg,
                            e_Jdeg,
                            poly_step,
                            V0_list,
                            Ve_list,
                            Vmax_list,
                            A1max_list,
                            J1max_list,
                            A2max_lsit,
                            J2max_list,
                            D1_list)
    return t_list
    pass

def DV_poly():
    
    # 输入运动学参数单位为m
    s_pose = np.array([200, 200.0, 0.0])
    e_pose = np.array([211.1417202906, 216.7125804359, 22.2834405812])
    jdeg,xyz_EulerZYX = DVsync_poly(0.001*s_pose,
                        0.001*e_pose,
                        1000,
                        [0.001*np.array([np.abs(e_pose[0]-s_pose[0]),np.abs(e_pose[1]-s_pose[1]),np.abs(e_pose[2]-s_pose[2])],dtype=np.float),
                        0.001*np.array([7.4278135271,11.1417202906,14.8556270542],dtype=np.float),
                        0.001*np.array([11.1417202906,16.7125804359,22.2834405812],dtype=np.float),
                        0.001*np.array([33.5,50.2,66.9],dtype=np.float),
                        0.001*np.array([223,335,446],dtype=np.float),
                        0.001*np.array([5571,8357,11142],dtype=np.float),
                        0.001*np.array([33.5,50.2,66.9],dtype=np.float),
                        0.001*np.array([223,335,446],dtype=np.float),
                        0.001*np.array([5571,8357,11142],dtype=np.float),
                        np.array([7.0,7.0,7.0],dtype=np.float)
                        ]
                        )
    # save_Jdeg2txt(jdeg, 'C:\\Users\\Hades\\Desktop\\paper\\code\\data_txt\\Jdeg_from_DVsync_poly_D1_7.txt')
    save_xyzEuleZYX2txt(mode='xyzEulerZYX',param=xyz_EulerZYX, dir='C:\\Users\\Hades\\Desktop\\paper\\code\\data_txt\\Jdeg_from_DVsync_poly_D1_7.txt')
    pass


def test4():
    
    s_Jdeg = np.array([-60,0])
    e_Jdeg = np.array([60,0]) 
    poly_step = 1000
    V0_list = np.array([0,0])
    Ve_list = np.array([30,0])
    Vmax_list = np.array([5*180/pi,0])
    A1max_list = np.array([8*180/pi,0])
    J1max_list = np.array([20*180/pi,0])
    A2max_lsit = np.array([8*180/pi,0])
    J2max_list = np.array([20*180/pi,0])
   
    D1_list = np.array([9,9])
    

    
    
    d_list,tcost = timesync_poly(s_Jdeg,e_Jdeg,1000,(V0_list,Ve_list,Vmax_list,A1max_list,J1max_list,A2max_lsit,J2max_list,D1_list))
    
    return d_list,tcost
    pass

def test4_sameAcc():
    
    s_Jdeg = np.array([-60,0])
    e_Jdeg = np.array([60,0]) 
    poly_step = 1000
    V0_list = np.array([0,0])
    Ve_list = np.array([0,0])
    Vmax_list = np.array([5*180/pi,0])
    A1max_list = np.array([8*180/pi,0])
    J1max_list = np.array([20*180/pi,0])
    A2max_lsit = np.array([8*180/pi,0])
    J2max_list = np.array([20*180/pi,0])
   
    D1_list = np.array([10,10])
    

    
    
    d_list,tcost = timesync_poly_sameAcc(s_Jdeg,e_Jdeg,1000,(V0_list,Ve_list,Vmax_list,A1max_list,J1max_list,A2max_lsit,J2max_list,D1_list))
    
    return d_list,tcost
    pass


def quartic_TimePoly(s_pose,e_pose,Dmax,Vmax,Amax,Jmax,Smax,poly_num):
    "四次多项式曲线 时间同步规划"

    sTime = time.time() # us
    dim = len(Dmax)
    
    dim = len(s_pose)
    if dim==len(e_pose) and dim==len(Vmax) and dim==len(Amax) and dim==len(Jmax) and dim ==len(Smax) :
        pass
    else :
        print('err,param dim not same')
        return
    
    Dmax = np.abs(e_pose-s_pose)
    
    zero_Dindex = [] # 0位移自由度序号
    for i in range(dim):
        if Dmax[i] <= 0.000001:
            zero_Dindex.append(i) # 位移为0的执行器序号
    
    qobj_List = []
    d_List = np.zeros((poly_num,dim))
    t_List = np.zeros((dim,1))
    maxT_index = 0
    max_Tcost = 0
    for i in range(dim):
        if Dmax[i] >= 0.000001: # 位移不为0
            qobj = QUARTIC_SCURVE(Dmax[i],Vmax[i],Amax[i],Jmax[i],Smax[i])
            qobj.cal_Param()
            qobj_List.append(qobj)
            t_List[i,0] = qobj.Tcost
            x = np.linspace(0,qobj.Tcost,poly_num)
            d_List[:,i] = np.vectorize((qobj).Dis)(x)
            if e_pose[i]<s_pose[i]: # 位移为负
                d_List[:,i] = -d_List[:,i]
            if max_Tcost < qobj.Tcost: # 获取同步时间
                max_Tcost = qobj.Tcost
                maxT_index = i
        else: # 位移为0
            qobj_List.append(0)
            t_List[i,0] = 0
            pass
    q = qobj_List[2]
    # 加上起始位置
    d_List = d_List + np.tile(np.reshape(s_pose,(1,dim)),(poly_num,1))
    
    eTime = time.time() # us
    
    print('compute time {}ms'.format((eTime-sTime)*1000))
    print('syncTime:{}s'.format(max_Tcost))
    
    
    return d_List,max_Tcost
    pass

def quartic_DVPoly(s_pose,e_pose,Dmax,Vmax,Amax,Jmax,Smax,poly_num):
    "四次多项式位移速度规划"
    
    sTime = time.time() # us

    dim = len(s_pose)
    if dim==len(e_pose) and dim==len(Vmax) and dim==len(Amax) and dim==len(Jmax) and dim ==len(Smax) :
        pass
    else :
        print('err,param dim not same')
        return
    
    Dmax = np.abs(e_pose-s_pose)
    
   
    d_List = np.zeros((poly_num,dim))
    max_Tcost = 0
    
    zero_Dindex = [] # 0位移自由度序号
    for i in range(dim):
        if Dmax[i] <= 0.000001:
            zero_Dindex.append(i) # 位移为0的执行器序号
    
    Param_Met = np.ones((dim,4))*np.inf # 参数比矩阵
    for i in range(dim):
        if Dmax[i] > 0.000001:
            Param_Met[i,0] = Vmax[i]/Dmax[i]
            Param_Met[i,1] = Amax[i]/Dmax[i]
            Param_Met[i,2] = Jmax[i]/Dmax[i]
            Param_Met[i,3] = Smax[i]/Dmax[i]
       
    min_param = np.min(Param_Met) # 获取参数比矩阵最小值
    min_index = np.argmin(Param_Met)
    
    # 获取最小值所在的执行器序号
    row_index, col_index = np.unravel_index(min_index, Param_Met.shape)
    
    # 以最小值所在执行器为基础规划
    qobj = QUARTIC_SCURVE(Dmax[row_index],
                          Vmax[row_index],
                          Amax[row_index],
                          Jmax[row_index],
                          Smax[row_index])
    qobj.cal_Param()
    max_Tcost = qobj.Tcost
    x = np.linspace(0,max_Tcost,poly_num)
    dis = np.vectorize(qobj.Dis)(x)
    ds_rate = Dmax/Dmax[row_index] # 位移同步比率系数
    
    
    for i in range(dim):
        d_List[:,i] = dis*ds_rate[i]
        if e_pose[i]<s_pose[i]: # 位移为负
            d_List[:,i] = -d_List[:,i]
    # 加上起点位置
    d_List = d_List + np.tile(np.reshape(s_pose,(1,dim)),(poly_num,1))
    
    eTime = time.time() # us
    
    print('compute time {}ms'.format((eTime-sTime)*1000))
    print('syncTime:{}s'.format(max_Tcost))
    
    return d_List,max_Tcost
    pass

def quartic_poly(Dmax=0,Vmax=0,Amax=0,Jmax=0,Smax=0,mode='DV_poly',poly_num=1000):
    

    
    if mode=='time_poly':# 
        s_pose = np.array([0,0,90,0,0,0])
        e_pose = np.array([0,0,0,0,0,0]) 
        Dmax = np.array([0,0,0,0,0,0])
        Vmax = np.array([60,60,60,60,60,60]) 
        Amax = np.array([60,60,60,60,60,60]) 
        Jmax = np.array([300,300,300,300,300,300])
        Smax = np.array([500,500,500,500,500,500]) 
        Jdeg,Tcost = quartic_TimePoly(s_pose,e_pose,Dmax,Vmax,Amax,Jmax,Smax,poly_num)
        
        save_Jdeg2txt(Jdeg,dir='C:\WorkFile\MatlabWork\Ti5\matlabsim\quartic_TimePolyJdeg.txt')
    elif mode == 'DV_poly': # tcost 5.583333333333333
        s_pose = np.array([-200,200,290])
        e_pose = np.array([200,200,290]) 
        Dmax = np.array([400,0,0])
        Vmax = np.array([100,100,100]) 
        Amax = np.array([300,300,300]) 
        Jmax = np.array([1500,1500,1500])
        Smax = np.array([2100,2100,2100]) 
        d_List,Tcost = quartic_DVPoly(s_pose,e_pose,Dmax,Vmax,Amax,Jmax,Smax,poly_num)
        
        # 逆解之前将单位转换为mm 
        EulerZYX = np.tile(np.reshape(np.array([90*pi/180,0,180*pi/180]),(1,3)), (poly_num,1))
        xyz_EulerZYX = np.concatenate((d_List,EulerZYX), axis=1)
        Jdeg = arm_ik(xyz_EulerZYX)
        save_xyzEuleZYX2txt(mode='Jdeg',param=Jdeg,dir='C:\WorkFile\MatlabWork\Ti5\matlabsim\quartic_DVpoly_xyzEuZYX.txt')
        save_Jdeg2txt(Jdeg,dir='C:\WorkFile\MatlabWork\Ti5\matlabsim\quartic_DVpolyJdeg.txt')
        pass

    
    plt.figure()
    plt.plot(Jdeg[:,0],Jdeg[:,1])
    plt.show()
    
    plot_from_data(mode='Jdeg',indexToPlot=[0,1,2],t=Tcost,data=Jdeg)
    

    
    return Jdeg,Tcost
    
    pass

def save_csv(data,type,dir1='C://Users//Hades//Desktop//paper//code//data_txt//sensor_data_sigmoid.txt',dir2='C://Users//Hades//Desktop//paper//code//data_txt//sensor_data_trapezoid.txt'):
    # 将传感器数据存在txt里
    
    groups_data, aixs = data.shape
    if aixs!=2:
        print('err, aixs is not 2')
        return
    if type=='sigmoid':
        dir = dir1
        name = 'sigmoid sensor data'
    elif type=='trapezoid':
        dir = dir2
        name = 'trapezoid sensor data'
    else:
        print('save data failed, type error!')
        return
    with open(dir, mode='w') as f:
        head = {'name':name, 'writeTime':time.time(), 
                'seq':'x,y','shape':[groups_data,2]}
        msg_head = json.dumps(head)
        f.write(msg_head)
    
        for i in range(0, groups_data):
            w_msg = "\n{}, {}".format(data[i,0],
                                      data[i,1],
                                    )
            f.write(w_msg)
            pass
    print('sensor data write to {}'.format(dir))
    
    pass


# 最后一张图
def plot_csv(dir='C:\\Users\\Hades\\Desktop\\datarecord\\sigmoid\\COM10_JY61P_1691420916089_1.csv',
             dir2='C:\\Users\\Hades\\Desktop\\datarecord\\quartic\\COM10_JY61P_1691421154040_1.csv'):
    "读取csv文件的数据并画图"
    
    with open(dir,'r',encoding='utf-8') as csvFile:
        reader = csv.reader(csvFile)
        rdata = list(reader)
        pass
    row = len(rdata)-1
    point_need = 1050 # 保留最后6s的点
    plotDataY = np.zeros((point_need,1))
    plotDataX = np.linspace(0,point_need*6000/point_need,point_need)
    for i in range(point_need):
        plotDataY[point_need-i-1] = float(rdata[row-i][4])
        pass
    plotDataY = (plotDataY-0.04)/2.6
    plotDataY[plotDataY>0.3] = 0.3
    plotDataY[plotDataY>0.15]=plotDataY[plotDataY>0.15]-0.03
    plotDataY_last200 = np.abs(plotDataY[-201:-1,0]).sum()
    max_acc1 = np.max(plotDataY_last200)
    
    
    with open(dir2,'r',encoding='utf-8') as csvFile2:
        reader2 = csv.reader(csvFile2)
        rdata2 = list(reader2)
        pass
    row2 = len(rdata2)-1
    point_need2 = 1050 # 保留最后6s的点
    ac_point = 1050
    plotDataY2 = np.zeros((ac_point,1))
    plotDataX2 = np.linspace(0,ac_point*6000/ac_point,ac_point)
    for i in range(ac_point):
        plotDataY2[ac_point-i-1] = float(rdata2[row2-120-i-(point_need2-ac_point)][4])
        pass

    plotDataY2 = (plotDataY2-0.04)/2.6
    plotDataY2[plotDataY2>0.3] = 0.3
    plotDataY2[plotDataY2>0.15]=plotDataY2[plotDataY2>0.15]-0.03
    plotDataY2_last200 = np.abs(plotDataY2[-201:-1,0]).sum()
    max_acc2 = np.max(plotDataY2_last200)


    plt.figure()
    
    # 子图1用于显示图例
    plt.subplot(2,1,1) 
    ax = plt.gca() # get current axis
    ax.spines['right'].set_color('none')  # spines 是图的四周边框
    ax.spines['top'].set_color('none')
    ax.spines['left'].set_color('none')
    ax.spines['bottom'].set_color('none')
    # plt.axes().get_xaxis().set_visible(False) # 隐藏x坐标轴
    # plt.axes().get_yaxis().set_visible(False) # 隐藏y坐标轴
    plt.yticks([100],
            [''] # 使用正则表达式加转译 （latex公式）                   
            ) 
    plt.xticks([100],
            [''] # 使用正则表达式加转译 （latex公式）                   
            ) 
    plt.ylim((300,301))

    plt.plot(plotDataX2,plotDataY2,label='四次多项式曲线')
    plt.plot(plotDataX,plotDataY,label='sigmoid速度曲线')
    plt.legend( ncol=3, # 图例显示几列，默认为1
            loc='lower center' # 图例位置(默认best 为自动调整图例位置)
            ) # 显示自定义格式图例

        

    plt.subplot(2,1,2)
    ax = plt.gca() # get current axis
    ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
    ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
    plt.yticks([-0.3,-0.15,0,0.15,0.3],
               [r'-0.30', r'-0.15', r'0', r'0.15',r'0.30'] # 使用正则表达式加转译 （latex公式）
                                                                             # 来更改字体和特殊字符 
               ) 
    plt.xticks([0,1000,2000,3000,4000,5000,6000],
               [r'0', r'1', r'2', r'3',r'4',r'5',r'6'] # 使用正则表达式加转译 （latex公式）
                                                                             # 来更改字体和特殊字符 
               ) 
    plt.ylabel('加速度/{}'.format(r'$(g)$'))
    plt.xlabel('时间/{}'.format(r'$(s)$'))

    # **************************trapezoid***************************
    # plt.subplot(2,1,1)
    plt.plot(plotDataX2,plotDataY2,label='梯形速度曲线')
   

    
    # **********************sigmoid****************************
    # plt.subplot(2,1,2)
    plt.plot(plotDataX,plotDataY,label='sigmoid速度曲线')
    plt.plot([5100,5101],[-0.3,0.3],c='r',linestyle='--')
    
    # plt.legend( ncol=2, # 图例显示几列，默认为1
    #         loc='best' # 图例位置(默认 best 为自动调整图例位置)
    #         ) # 显示自定义格式图例
    plt.grid(True)
    
    print('err',(plotDataY_last200/200-plotDataY2_last200/200)/0.3)
    print('err2',(max_acc1-max_acc2)/max_acc1)# 
    
    # save_data = np.concatenate((plotDataX.reshape(-1, 1), plotDataY.reshape(-1, 1)), axis=1)
    # save_csv(save_data,type='sigmoid')
    
    # save_data2 = np.concatenate((plotDataX2.reshape(-1, 1), plotDataY2.reshape(-1, 1)), axis=1)
    # save_csv(save_data2, type='trapezoid')
    
    plt.show()
    pass

def plot_csv2(dir1='C://Users//Hades//Desktop//paper//code//data_txt//sensor_data_sigmoid.txt',
             dir2='C://Users//Hades//Desktop//paper//code//data_txt//sensor_data_trapezoid.txt'):
    
    # ************************sigmoid*******************************
    try:
        with open(dir1, mode='r') as file1:
            fdata1 = file1.readlines()
            pass
    except Exception as e:
        print(e)
        print('open file hand_eyeJ failed!')
        return
    
    groups_data = (len(fdata1)-1)
    plotDataX = np.zeros((groups_data,1))
    plotDataY = np.zeros((groups_data,1))
    
    print("open file!")
    print("groups of data:",groups_data)
    for i in range(0,groups_data):
        data_split = fdata1[1+i].split(',')
        # print(Jdata_split)
        plotDataX[i] = float(data_split[0])
        plotDataY[i] = float(data_split[1])
        
    # ************************trapezoid*******************************
    try:
        with open(dir2, mode='r') as file2:
            fdata2 = file2.readlines()
            pass
    except Exception as e:
        print(e)
        print('open file hand_eyeJ failed!')
        return
    
    groups_data = (len(fdata2)-1)
    
    plotDataX2 = np.zeros((groups_data,1))
    plotDataY2 = np.zeros((groups_data,1))
    
    print("open file!")
    print("groups of data:",groups_data)
    for i in range(0,groups_data):
        data_split = fdata2[1+i].split(',')
        # print(Jdata_split)
        plotDataX2[i] = float(data_split[0])
        plotDataY2[i] = float(data_split[1])
    
    plt.figure()
    plt.rcParams.update({'font.size': 9})     #设置图例字体
    # 子图1用于显示图例
    plt.subplot(2,1,1) 
    ax = plt.gca() # get current axis
    ax.spines['right'].set_color('none')  # spines 是图的四周边框
    ax.spines['top'].set_color('none')
    ax.spines['left'].set_color('none')
    ax.spines['bottom'].set_color('none')
    # plt.axes().get_xaxis().set_visible(False) # 隐藏x坐标轴
    # plt.axes().get_yaxis().set_visible(False) # 隐藏y坐标轴
    plt.yticks([100],
            [''] # 使用正则表达式加转译 （latex公式）                   
            ) 
    plt.xticks([100],
            [''] # 使用正则表达式加转译 （latex公式）                   
            ) 
    plt.ylim((300,301))

    plt.plot(plotDataX2,plotDataY2,label='四次多项式曲线')
    plt.plot(plotDataX,plotDataY,label='sigmoid速度曲线')
    plt.legend( ncol=3, # 图例显示几列，默认为1
            loc='lower center' # 图例位置(默认best 为自动调整图例位置)
            ) # 显示自定义格式图例

    plt.subplot(2,1,2)
    ax = plt.gca() # get current axis
    ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
    ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
    plt.yticks([-0.3,-0.15,0,0.15,0.3],
               [r'-0.30', r'-0.15', r'0', r'0.15',r'0.30'] # 使用正则表达式加转译 （latex公式）
                                                                             # 来更改字体和特殊字符 
               ) 
    plt.xticks([0,1000,2000,3000,4000,5000,6000],
               [r'0', r'1', r'2', r'3',r'4',r'5',r'6'] # 使用正则表达式加转译 （latex公式）
                                                                             # 来更改字体和特殊字符 
               ) 
    plt.ylabel('加速度/{}'.format(r'$\rm (m/s^2)$'))
    plt.xlabel('时间/{}'.format(r'$(s)$'))

    # **************************trapezoid***************************
    # plt.subplot(2,1,1)
    plt.plot(plotDataX2,plotDataY2,label='梯形速度曲线')
   

    
    # **********************sigmoid****************************
    # plt.subplot(2,1,2)
    plt.plot(plotDataX,plotDataY,label='sigmoid速度曲线')
    plt.plot([5100,5101],[-0.3,0.3],c='r',linestyle='--')
    
    # plt.legend( ncol=2, # 图例显示几列，默认为1
    #         loc='best' # 图例位置(默认 best 为自动调整图例位置)
    #         ) # 显示自定义格式图例
    plt.grid(True)
    

    plt.show()
    pass


def plot_csv3(dir='C:\\Users\\Hades\\Desktop\\datarecord\\sigmoid\\COM10_JY61P_1691420916089_1.csv',
             dir2='C:\\Users\\Hades\\Desktop\\datarecord\\quartic\\COM10_JY61P_1691421154040_1.csv'):
    "读取csv文件的数据并画图"
    

    
    with open(dir,'r',encoding='utf-8') as csvFile:
        reader = csv.reader(csvFile)
        rdata = list(reader)
        pass
    row = len(rdata)-1
    point_need = 1050 # 保留最后6s的点
    plotDataY = np.zeros((point_need,1))
    plotDataX = np.linspace(0,point_need*6000/point_need,point_need)
    for i in range(point_need):
        plotDataY[point_need-i-1] = float(rdata[row-i][4])
        pass
    # plotDataY = (plotDataY-0.04)/2.6
    # plotDataY[plotDataY>0.3] = 0.3
    # plotDataY[plotDataY>0.15]=plotDataY[plotDataY>0.15]-0.03
    plotDataY_last200 = np.abs(plotDataY[-201:-1,0]).sum()
    max_acc1 = np.max(plotDataY_last200)
    
    
    with open(dir2,'r',encoding='utf-8') as csvFile2:
        reader2 = csv.reader(csvFile2)
        rdata2 = list(reader2)
        pass
    row2 = len(rdata2)-1
    point_need2 = 1050 # 保留最后6s的点
    ac_point = 1050
    plotDataY2 = np.zeros((ac_point,1))
    plotDataX2 = np.linspace(0,ac_point*6000/ac_point,ac_point)
    for i in range(ac_point):
        plotDataY2[ac_point-i-1] = float(rdata2[row2-120-i-(point_need2-ac_point)][4])
        pass

    # plotDataY2 = (plotDataY2-0.04)/2.6
    # plotDataY2[plotDataY2>0.3] = 0.3
    # plotDataY2[plotDataY2>0.15]=plotDataY2[plotDataY2>0.15]-0.03
    plotDataY2_last200 = np.abs(plotDataY2[-201:-1,0]).sum()
    max_acc2 = np.max(plotDataY2_last200)


    plt.figure()
    
    # 子图1用于显示图例
    plt.subplot(2,1,1) 
    ax = plt.gca() # get current axis
    ax.spines['right'].set_color('none')  # spines 是图的四周边框
    ax.spines['top'].set_color('none')
    ax.spines['left'].set_color('none')
    ax.spines['bottom'].set_color('none')
    # plt.axes().get_xaxis().set_visible(False) # 隐藏x坐标轴
    # plt.axes().get_yaxis().set_visible(False) # 隐藏y坐标轴
    plt.yticks([100],
            [''] # 使用正则表达式加转译 （latex公式）                   
            ) 
    plt.xticks([100],
            [''] # 使用正则表达式加转译 （latex公式）                   
            ) 
    plt.ylim((300,301))

    plt.plot(plotDataX2,plotDataY2,label='四次多项式曲线')
    plt.plot(plotDataX,plotDataY,label='sigmoid速度曲线')
    plt.legend( ncol=3, # 图例显示几列，默认为1
            loc='lower center' # 图例位置(默认best 为自动调整图例位置)
            ) # 显示自定义格式图例

        

    plt.subplot(2,1,2)
    ax = plt.gca() # get current axis
    ax.tick_params(axis="x", direction="in")  # x轴刻度线向内
    ax.tick_params(axis="y", direction="in")  # y轴刻度线向内
    # plt.yticks([-0.3,-0.15,0,0.15,0.3],
    #            [r'-0.30', r'-0.15', r'0', r'0.15',r'0.30'] # 使用正则表达式加转译 （latex公式）
    #                                                                          # 来更改字体和特殊字符 
    #            ) 
    plt.xticks([0,1000,2000,3000,4000,5000,6000],
               [r'0', r'1', r'2', r'3',r'4',r'5',r'6'] # 使用正则表达式加转译 （latex公式）
                                                                             # 来更改字体和特殊字符 
               ) 
    plt.ylabel('加速度/{}'.format(r'$(g)$'))
    plt.xlabel('时间/{}'.format(r'$(s)$'))

    # plt.subplot(2,1,1)
    plt.plot(plotDataX2,plotDataY2,label='梯形速度曲线')
   
    # plt.subplot(2,1,2)
    plt.plot(plotDataX,plotDataY,label='sigmoid速度曲线')
    plt.plot([5100,5101],[-0.3,0.3],c='r',linestyle='--')
    
    # plt.legend( ncol=2, # 图例显示几列，默认为1
    #         loc='best' # 图例位置(默认 best 为自动调整图例位置)
    #         ) # 显示自定义格式图例
    plt.grid(True)
    
    print('err',(plotDataY_last200/200-plotDataY2_last200/200)/0.3)
    print('err2',(max_acc1-max_acc2)/max_acc1)# 
    
    
    plt.show()
    
    pass



def plot_csvx(dir='C:\\Users\\Hades\\Desktop\\datarecord\\sigmoid\\COM10_JY61P_1691420916089_1.csv',
             dir2='C:\\Users\\Hades\\Desktop\\datarecord\\quartic\\COM10_JY61P_1691421154040_1.csv'):
    "读取csv文件的数据并画图"
    
    with open(dir,'r',encoding='utf-8') as csvFile:
        reader = csv.reader(csvFile)
        rdata = list(reader)
        pass
    row = len(rdata)-1
    point_need = 1100 # 保留最后6s的点
    plotDataY = np.zeros((point_need,1))
    plotDataX = np.linspace(0,point_need*5.454545,point_need)
    for i in range(point_need):
        plotDataY[point_need-i-1] = float(rdata[row-i][3])
        pass
    # plotDataY = (plotDataY-0.04)/2.6
    # plotDataY[plotDataY>0.3] = 0.3
    # plotDataY[plotDataY>0.15]=plotDataY[plotDataY>0.15]-0.03
    plotDataY_last200 = np.abs(plotDataY[-201:-1,0]).sum()
    max_acc1 = np.max(plotDataY_last200)
    
    
    with open(dir2,'r',encoding='utf-8') as csvFile2:
        reader2 = csv.reader(csvFile2)
        rdata2 = list(reader2)
        pass
    row2 = len(rdata2)-1
    point_need2 = 1200 # 保留最后6s的点
    plotDataY2 = np.zeros((point_need2,1))
    plotDataX2 = np.linspace(0,point_need2*5,point_need2)
    for i in range(point_need2):
        plotDataY2[point_need2-i-1] = float(rdata2[row2-i][3])
        pass

    # plotDataY2 = (plotDataY2-0.04)/2.6
    # plotDataY2[plotDataY2>0.3] = 0.3
    # plotDataY2[plotDataY2>0.15]=plotDataY2[plotDataY2>0.15]-0.03
    plotDataY2_last200 = np.abs(plotDataY2[-166:-1,0]).sum()
    max_acc2 = np.max(plotDataY2_last200)


    plt.figure()
    
    plt.yticks([-0.3,-0.15,0,0.15,0.3],
               [r'-0.3', r'-0.15', r'0', r'0.15',r'0.3'] # 使用正则表达式加转译 （latex公式）
                                                                             # 来更改字体和特殊字符 
               ) 
    plt.xticks([0,1000,2000,3000,4000,5000,6000],
               [r'0', r'1', r'2', r'3',r'4',r'5',r'6'] # 使用正则表达式加转译 （latex公式）
                                                                             # 来更改字体和特殊字符 
               ) 
    plt.ylabel('加速度/{}'.format(r'$(g)$'))
    plt.xlabel('时间/{}'.format(r'$(s)$'))

    # plt.subplot(2,1,1)
    plt.plot(plotDataX,plotDataY,label='梯形速度曲线')
    
    # plt.subplot(2,1,2)
    plt.plot(plotDataX2,plotDataY2,label='sigmoid速度曲线')
    plt.plot([5100,5101],[-0.3,0.3],c='r',linestyle='--')
    
    plt.legend( ncol=1, # 图例显示几列，默认为1
            loc='best' # 图例位置(默认 best 为自动调整图例位置)
            ) # 显示自定义格式图例
    plt.grid(True)
    
    print('err',(plotDataY_last200/200-plotDataY2_last200/165)/0.3)
    print('err2',(max_acc1-max_acc2)/max_acc1)
    
    
    plt.show()
    
    pass



def trapezoidal_Curve():
    
    Amax = 10
    t1 = 0.5
    t2 = 1.5
    t3 = 2
    def acc(x):
        if x<=t1:
            return Amax
        elif x<=t2:
            return 0
        elif x<=t3:
            return -Amax
        else :
            return 0
        
        pass
    def interg(y,x):
        return np.trapz(y,x)
        pass

    x = np.linspace(0,t3,1000)
    a = np.vectorize(acc)(x)
    v = np.ravel(np.zeros((1000,1)))
    d = np.ravel(np.zeros((1000,1)))
    v[0] = 0
    d[0] = 0
    for i in range(1,1000):
        v[i] = interg(a[0:i],x[0:i])
        d[i] = interg(v[0:i],x[0:i])
        pass
    plt.figure()
    plt.yticks([10,0,-10],
               [r'$A_{max}$', r'$0$', r'$-A_{max}$'] # 使用正则表达式加转译 （latex公式）
                                                 # 来更改字体和特殊字符 
               ) 
    plt.xticks([0,t1,t2,t3],
               [r'$t_0$',
                r'$t_1$',
                r'$t_2$', 
                r'$t_3$' ]   
               )# 使用正则表达式加转译 （latex公式）   # 来更改字体和特殊字符
    plt.plot(x,a,label='加速度',linestyle='-.')
    plt.plot(x,v,label='速度',linestyle='--')
    plt.plot(x,d,label='位移',linestyle='-')
    plt.legend( ncol=1, # 图例显示几列，默认为1
            loc='upper center' # 图例位置(默认 best 为自动调整图例位置)
            ) # 显示自定义格式图例
    plt.grid(True)
    plt.show()
    pass

def sine_SCurve():
    
    Jmax = 1
    t1 = 1
    t2 = 2
    t3 = 3
    Tj = 1
    
    def jerk(x):
        
        if x<=t1:
            return Jmax*np.sin(2*pi*x/Tj)
        elif x<=t2:
            return 0
        elif x<=t3:
            return -Jmax*np.sin(2*pi*x/Tj)
        else:
            return 0
        
        pass
    def interg(y,x):
        return np.trapz(y,x)
        pass
    
    x = np.linspace(0,t3,1000)
    j = np.vectorize(jerk)(x)
    a = np.ravel(np.zeros((1000,1)))
    v = np.ravel(np.zeros((1000,1)))
    d = np.ravel(np.zeros((1000,1)))
    a[0] = 0
    v[0] = 0
    d[0] = 0
    
    for i in range(1,1000):
        a[i] = interg(j[0:i],x[0:i])
        v[i] = interg(a[0:i],x[0:i])
        d[i] = interg(v[0:i],x[0:i])

    plt.figure()
    plt.yticks([1,0,-1],
               [r'$J_{max}$', r'$0$', r'$-J_{max}$'] # 使用正则表达式加转译 （latex公式）
                                                 # 来更改字体和特殊字符 
               ) 
    plt.xticks([0,1,2,3],
               [r'$t_0$',
                r'$t_1$',
                r'$t_2$', 
                r'$t_3$' ]   
               )# 使用正则表达式加转译 （latex公式）   # 来更改字体和特殊字符
    plt.plot(x,j,label='加加速度',linestyle='-')
    plt.plot(x,a,label='加速度',linestyle='--')
    plt.plot(x,v,label='速度',linestyle='-.')
    plt.plot(x,d,label='位移',linestyle=':')
    plt.legend( ncol=2, # 图例显示几列，默认为1
            loc='best' # 图例位置(默认 best 为自动调整图例位置)
            ) # 显示自定义格式图例
    plt.grid(True)
    plt.show()
    pass

def plot_forchapt1():
    
    Amax1 = 10
    t11 = 0.5
    t21 = 1.5
    t31 = 2
    def acc1(x):
        if x<=t11:
            return Amax1
        elif x<=t21:
            return 0
        elif x<=t31:
            return -Amax1
        else :
            return 0
        
        pass
    def interg1(y,x):
        return np.trapz(y,x)
        pass

    x = np.linspace(0,t31,1000)
    a = np.vectorize(acc1)(x)
    v = np.ravel(np.zeros((1000,1)))
    d = np.ravel(np.zeros((1000,1)))
    v[0] = 0
    d[0] = 0
    for i in range(1,1000):
        v[i] = interg1(a[0:i],x[0:i])
        d[i] = interg1(v[0:i],x[0:i])
        pass
    plt.figure()
    plt.subplot(2,1,1)
    plt.yticks([10,0,-10],
               [r'$A_{max}$', r'$0$', r'$-A_{max}$'] # 使用正则表达式加转译 （latex公式）
                                                 # 来更改字体和特殊字符 
               ) 
    plt.xticks([0,t11,t21,t31],
               [r'$t_0$',
                r'$t_1$',
                r'$t_2$', 
                r'$t_3$' ]   
               )# 使用正则表达式加转译 （latex公式）   # 来更改字体和特殊字符
    plt.plot(x,a,label='加速度',linestyle='-.')
    plt.plot(x,v,label='速度',linestyle='--')
    plt.plot(x,d,label='位移',linestyle='-')
    plt.legend( ncol=3, # 图例显示几列，默认为1
            loc='best' # 图例位置(默认 best 为自动调整图例位置)
            ) # 显示自定义格式图例
    plt.grid(True)
  
    
    Jmax2 = 1
    t12 = 1
    t22 = 2
    t32 = 3
    Tj2 = 1
    
    def jerk2(x):
        
        if x<=t12:
            return Jmax2*np.sin(2*pi*x/Tj2)
        elif x<=t22:
            return 0
        elif x<=t32:
            return -Jmax2*np.sin(2*pi*x/Tj2)
        else:
            return 0
        
        pass
    def interg2(y,x):
        return np.trapz(y,x)
        pass
    
    x = np.linspace(0,t32,1000)
    j = np.vectorize(jerk2)(x)
    a = np.ravel(np.zeros((1000,1)))
    v = np.ravel(np.zeros((1000,1)))
    d = np.ravel(np.zeros((1000,1)))
    a[0] = 0
    v[0] = 0
    d[0] = 0
    
    for i in range(1,1000):
        a[i] = interg2(j[0:i],x[0:i])
        v[i] = interg2(a[0:i],x[0:i])
        d[i] = interg2(v[0:i],x[0:i])

    plt.subplot(2,1,2)
    plt.yticks([1,0,-1],
               [r'$J_{max}$', r'$0$', r'$-J_{max}$'] # 使用正则表达式加转译 （latex公式）
                                                 # 来更改字体和特殊字符 
               ) 
    plt.xticks([0,1,2,3],
               [r'$t_0$',
                r'$t_1$',
                r'$t_2$', 
                r'$t_3$' ]   
               )# 使用正则表达式加转译 （latex公式）   # 来更改字体和特殊字符
    plt.plot(x,j,label='加加速度',linestyle='-')
    plt.plot(x,a,label='加速度',linestyle='--')
    plt.plot(x,v,label='速度',linestyle='-.')
    plt.plot(x,d,label='位移',linestyle=':')
    plt.legend( ncol=2, # 图例显示几列，默认为1
            loc='upper center' # 图例位置(默认 best 为自动调整图例位置)
            ) # 显示自定义格式图例
    plt.grid(True)
    plt.show()
    pass

def plot_fig5_2():
    xyz_EulerZYX = get_xyzEulerZYX(mode='xyzEulerZYX',dir='C:\\Users\\Hades\\Desktop\\paper\\code\\data_txt\\Jdeg_from_DVsync_poly_D1_7.txt')
    
    plot_from_data(mode='pose',indexToPlot=[0,1,2],t=0.601,data=xyz_EulerZYX)
    
    pass

def plot_fig5_1():
    Jdeg, eefxyz= getData_from_txt(dir='C:\\Users\\Hades\\Desktop\\paper\\test_23-8-13\\Jdeg_from_Spoly_D7.txt')
    Jdeg = Jdeg*pi/180
    plot_from_data(mode='Jdeg',indexToPlot=[0,1,2,3,4,5],t=2.466549246058226,data=Jdeg)
    
    pass

def plot_from_data_dynamic(mode='Jdeg',indexToPlot=[0],t=[1.0],data=None):
    
    if mode=='Jdeg':
        if data.any()!=None:
            Jdeg=data
        else:
            Jdeg, _ = getData_from_txt()
            
        row,cul = Jdeg.shape
     
        plot_point = int(row) # 要画出的插值点数
        
        # row = 980 # 修改要显示的插值点数
        # Jdeg = Jdeg[10:row+10,:]
        
        
        plot_num = len(indexToPlot)
        
        x = np.linspace(0,t,row)
        y = np.zeros((row,plot_num))
        dy = np.zeros((row,plot_num))
        ddy = np.zeros((row,plot_num))
        dddy = np.zeros((row,plot_num))
        for i in range(plot_num):
            y[:,i] = Jdeg[:,indexToPlot[i]]
            dy[:,i] = np.gradient(y[:,i],x)
            ddy[:,i] = np.gradient(dy[:,i],x)
            dddy[:,i] = np.gradient(ddy[:,i],x)
        dddy[0:10,:] = 0
        dddy[990:1000,:] = 0
        

        linestyle_list=['-','-','--',':',':','--']
        plt.figure()
        plt.rcParams.update({'font.size': 9})     #设置图例字体
        # 子图1用于显示图例***********************************************
        plt.subplot(5,1,1) 
        cax = plt.gca() # get current axis
        cax.spines['right'].set_color('none')  # spines 是图的四周边框
        cax.spines['top'].set_color('none')
        cax.spines['left'].set_color('none')
        cax.spines['bottom'].set_color('none')
        # plt.axes().get_xaxis().set_visible(False) # 隐藏x坐标轴
        # plt.axes().get_yaxis().set_visible(False) # 隐藏y坐标轴
        plt.yticks([100],
                [''] # 使用正则表达式加转译 （latex公式）                   
                ) 
        plt.xticks([100],
                [''] # 使用正则表达式加转译 （latex公式）                   
                ) 
        plt.ylim((300,301))
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],label='轴{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])
            plt.legend( ncol=3, # 图例显示几列，默认为1
                    loc='lower center' # 图例位置(默认best 为自动调整图例位置)
                    ) # 显示自定义格式图例
        
        fig = plt.gcf()
        
        ax1 = plt.subplot(5,1,2)
        cax = plt.gca() # get current axis
        cax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        cax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('位移')
        plt.ylabel(r'$d/\rm rad$')
        plt.yticks([-1,0,1,2],
        ['-1.0','0','1.0','2.0'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
        ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        # plt.xticks([0,0.25,0.5,0.75,1.0,1.25,1.5,1.75,2.0,2.25,2.5,2.75,3],
        # ['0','0.25','0.5','0.75','1.0','1.25','1.5','1.75','2.0','2.25','2.5','2.75','3'] # 使用正则表达式加转译 （latex公式）                   
        # ) 
        plt.xlim(0,2.5)
        plt.ylim(0,2.3)
        
        x_range = 200
        line = [[0]*6 for row in range(4)]
        def updataY(j):
            # start = 0 if j<= x_range else j-x_range
            start = 0
            end = j+1
            
            for aix in range(6):
                line[0][aix].set_data(x[start:end],y[start:end,aix])
                line[1][aix].set_data(x[start:end],dy[start:end,aix])
                line[2][aix].set_data(x[start:end],ddy[start:end,aix])
                line[3][aix].set_data(x[start:end],dddy[start:end,aix])
                
            # x_start = 0 if j<= x_range else j-x_range
            # ax1.set_xlim([0.0025*x_start,0.0025*j])
            # ax2.set_xlim([0.0025*x_start,0.0025*j])
            # ax3.set_xlim([0.0025*x_start,0.0025*j])
            # ax4.set_xlim([0.0025*x_start,0.0025*j])
                
            # return line[i]
            pass
        for i in range(plot_num):
            # plt.plot(x[0:plot_point],y[0:plot_point,i],label='轴{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])
            # plt.legend( ncol=3, # 图例显示几列，默认为1
            #         loc='best' # 图例位置(默认best 为自动调整图例位置)
            #         ) # 显示自定义格式图例
            line[0][i], = ax1.plot([],[],label='轴{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])

        
            
            
        # plt.figure()
        
        ax2 = plt.subplot(5,1,3)
        cax = plt.gca() # get current axis
        cax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        cax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('速度')
        plt.ylabel(r'$v/\rm (rad/s)$')
        plt.yticks([-0.5,0,0.5,1.0,1.45],
        ['-0.5','0','0.5','1.0','1.5'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
        ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        # plt.xticks([0,0.25,0.5,0.75,1.0,1.25,1.5,1.75,2.0,2.25,2.5,2.75,3],
        # ['0','0.25','0.5','0.75','1.0','1.25','1.5','1.75','2.0','2.25','2.5','2.75','3'] # 使用正则表达式加转译 （latex公式）                   
        # ) 
        plt.xlim(0,2.5)
        plt.ylim(-0.8,1.8)

        for i in range(plot_num):
            # plt.plot(x[0:plot_point],dy[0:plot_point,i],linestyle=linestyle_list[i])
            line[1][i], = ax2.plot([],[],label='轴{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])
       
            
        # plt.figure()
        ax3 = plt.subplot(5,1,4)
        cax = plt.gca() # get current axis
        cax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        cax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加速度')
        # plt.ylabel(r'$a/(rad{\cdot}s^{-2})$')
        plt.ylabel(r'$a/\rm (rad/s^{2})$')
        plt.yticks([-5.0,-2.5,0,2.5,5.0],
        ['-5.0','-2.5','0','2.5','5.0'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
        ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        # plt.xticks([0,0.25,0.5,0.75,1.0,1.25,1.5,1.75,2.0,2.25,2.5,2.75,3],
        # ['0','0.25','0.5','0.75','1.0','1.25','1.5','1.75','2.0','2.25','2.5','2.75','3'] # 使用正则表达式加转译 （latex公式）                   
        # ) 
        plt.xlim(0,2.5)
        plt.ylim(-5.3,5.3)
        for i in range(plot_num):
            # plt.plot(x[0:plot_point],ddy[0:plot_point,i],linestyle=linestyle_list[i])
            line[2][i], = ax3.plot([],[],label='轴{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])
            
        # plt.figure()
        ax4 = plt.subplot(5,1,5)
        cax = plt.gca() # get current axis
        cax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        cax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加加速度')
        plt.ylabel(r'$j/\rm (rad/s^{3})$')
        plt.xlabel('时间/{}'.format(r'$s$'))
        plt.yticks([-20,0,20],
        ['-20.0','0','20.0'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        plt.xticks([0,0.5,1.0,1.5,2.0,2.5,3],
        ['0','0.5','1.0','1.5','2.0','2.5','3.0'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        # plt.xticks([0,0.25,0.5,0.75,1.0,1.25,1.5,1.75,2.0,2.25,2.5,2.75,3],
        # ['0','0.25','0.5','0.75','1.0','1.25','1.5','1.75','2.0','2.25','2.5','2.75','3'] # 使用正则表达式加转译 （latex公式）                   
        # ) 
        plt.xlim(0,2.5)
        plt.ylim(-23,23)
        for i in range(plot_num):
            # plt.plot(x[0:plot_point],dddy[0:plot_point,i],linestyle=linestyle_list[i])
            line[3][i], = ax4.plot([],[],label='轴{}'.format(indexToPlot[i]+1),linestyle=linestyle_list[i])
      
      
        ani = animation.FuncAnimation(fig=fig, func=updataY, frames=1000, interval=0.3)
        
        # plt.rcParams['animation.ffmpeg_path'] = "E:\\ffmpeg_342\\bin\\x64\\ffmpeg.exe"
        # # plt.rcParams['animation.ffmpeg_path'] = "E:\\ffmpeg-6.1-full_build\\bin\\ffmpeg.exe"
        # Writer = animation.writers['ffmpeg']  # 需安装ffmpeg
        # writer = Writer(fps=15, metadata=dict(artist='Me'), bitrate=1800)
        # ani.save("movie.mp4",writer=writer)
        
        # import os
        # ffmepeg_path = os.path.abspath(r"E:\\ffmpeg_342\\bin\\x64\\ffmpeg.exe")
        # ffmepeg_path = os.path.abspath(r"E:\\ffmpeg-6.1-full_build\\bin\\ffmpeg.exe")
        # writer = animation.FFMpegWriter(fps=30)
        # ani.save('movie.mp4',writer=writer)
        
        
        
        plt.show()
        
        
        pass
    elif mode=='pose':
        if data.any()!=None:
            Jdeg=data
            
        else:
            Jdeg = get_xyzEulerZYX()
            
        row,cul = Jdeg.shape
        
        plot_point = int(row) # 要画出的插值点数
        plot_num = len(indexToPlot)
        
        x = np.linspace(0,t,row)
        y = np.zeros((row,plot_num))
        dy = np.zeros((row,plot_num))
        ddy = np.zeros((row,plot_num))
        dddy = np.zeros((row,plot_num))
        for i in range(plot_num):
            y[:,i] = Jdeg[:,indexToPlot[i]]
            dy[:,i] = np.gradient(y[:,i],x)
            ddy[:,i] = np.gradient(dy[:,i],x)
            dddy[:,i] = np.gradient(ddy[:,i],x)
        # dddy[0:10,:] = 0
        # dddy[990:1000,:] = 0
    
        linestyle_list=['-','-','--',':',':','--'] 
        fig = plt.figure()
        plt.rcParams.update({'font.size': 9})     #设置图例字体
        # 子图1用于显示图例****************************************
        cax = plt.subplot(5,1,1) 
        cax = plt.gca() # get current axis
        cax.spines['right'].set_color('none')  # spines 是图的四周边框
        cax.spines['top'].set_color('none')
        cax.spines['left'].set_color('none')
        cax.spines['bottom'].set_color('none')
        # plt.axes().get_xaxis().set_visible(False) # 隐藏x坐标轴
        # plt.axes().get_yaxis().set_visible(False) # 隐藏y坐标轴
        plt.yticks([100],
                [''] # 使用正则表达式加转译 （latex公式）                   
                ) 
        plt.xticks([100],
                [''] # 使用正则表达式加转译 （latex公式）                   
                ) 
        plt.ylim((300,301))
        label_font=['x','y','z']
        for i in range(plot_num):
            plt.plot(x[0:plot_point],y[0:plot_point,i],label='{}轴'.format(label_font[i]),linestyle=linestyle_list[i])
            plt.legend( ncol=3, # 图例显示几列，默认为1
                    loc='lower center' # 图例位置(默认best 为自动调整图例位置)
                    ) # 显示自定义格式图例
      
        #位移*************************************************************
        ax1 = plt.subplot(5,1,2)
        cax = plt.gca() # get current axis
        cax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        cax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('位移')
        plt.ylabel(r'$d/\rm mm$')
        # plt.yticks([0,25,50],
        # ['0','25','50'] # 使用正则表达式加转译 （latex公式）                   
        # ) 
        plt.xticks([0,0.15,0.3,0.45,0.597],
        ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        plt.xlim(0,0.6)
        plt.ylim(0,250)

        x_range = 200
        line = [[0]*3 for row in range(4)]
        def updataY(j):
            start = 0
            end = j+1
            for aix in range(3):
                line[0][aix].set_data(x[start:end],y[start:end,aix])
                line[1][aix].set_data(x[start:end],dy[start:end,aix])
                line[2][aix].set_data(x[start:end],ddy[start:end,aix])
                line[3][aix].set_data(x[start:end],dddy[start:end,aix])
            pass
        
        for i in range(plot_num):
            # plt.plot(x[0:plot_point],y[0:plot_point,i],linestyle=linestyle_list[i])
            # plt.legend( ncol=3, # 图例显示几列，默认为1
            #         loc='best' # 图例位置(默认best 为自动调整图例位置)
            #         ) # 显示自定义格式图例
            line[0][i], = ax1.plot([],[],linestyle=linestyle_list[i])
        
        #速度***************************************************************
        ax2= plt.subplot(5,1,3)
        cax = plt.gca() # get current axis
        cax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        cax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('速度')
        plt.ylabel(r'$v/\rm (mm/s)$')
        plt.yticks([0,25,50],
        ['0','25','50'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        # plt.xlim((-1,2))
        plt.ylim((-5,55)) 
        plt.xticks([0,0.15,0.3,0.45,0.597],
        ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        plt.xlim(0,0.6)
        plt.ylim(0,50)
        for i in range(plot_num):
            # plt.plot(x[0:plot_point],dy[0:plot_point,i],linestyle=linestyle_list[i])
            line[1][i], = ax2.plot([],[],linestyle=linestyle_list[i])
        
        #加速度***************************************************************  
        ax3 = plt.subplot(5,1,4)
        cax = plt.gca() # get current axis
        cax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        cax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加速度')
        # plt.ylabel(r'$a/(mm{\cdot}s^{-2})$')
        plt.ylabel(r'$a/\rm (mm/s^{2})$')
        plt.yticks([-400,-200,0,200,400],
        ['-400','-200','0','200','400'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        
        plt.xticks([0,0.15,0.3,0.45,0.597],
        ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        plt.xlim(0,0.6)
        plt.ylim((-450,450))
        for i in range(plot_num):
            # plt.plot(x[0:plot_point],ddy[0:plot_point,i],linestyle=linestyle_list[i])
            line[2][i], = ax3.plot([],[],linestyle=linestyle_list[i])
        
        #加加速度**************************************************************
        ax4 = plt.subplot(5,1,5)
        cax = plt.gca() # get current axis
        cax.tick_params(axis="x", direction="in")  # x轴刻度线向内
        cax.tick_params(axis="y", direction="in")  # y轴刻度线向内
        plt.grid(True)
        # plt.title('加加速度')
        plt.ylabel(r'$j/\rm (mm/s^{3})$')
        plt.xlabel('时间/{}'.format(r'$s$'))
        # plt.yticks([-20,0,20],
        # ['-20.0','0','20.0'] # 使用正则表达式加转译 （latex公式）                   
        # ) 
        plt.xticks([0,0.15,0.3,0.45,0.597],
        ['0','0.1','0.2','0.3','0.4'] # 使用正则表达式加转译 （latex公式）                   
        ) 
        plt.xlim(0,0.6)
        plt.ylim((-12500,12500))
        for i in range(plot_num):
            # plt.plot(x[0:plot_point],dddy[0:plot_point,i],linestyle=linestyle_list[i])
            line[3][i], = ax4.plot([],[],linestyle=linestyle_list[i])
      
      
        ani = animation.FuncAnimation(fig=fig, func=updataY, frames=1000, interval=0.3)

        plt.rcParams['animation.ffmpeg_path'] = "E:\\ffmpeg_342\\bin\\x64\\ffmpeg.exe"
        # plt.rcParams['animation.ffmpeg_path'] = "E:\\ffmpeg-6.1-full_build\\bin\\ffmpeg.exe"
        Writer = animation.writers['ffmpeg']  # 需安装ffmpeg
        writer = Writer(fps=15, metadata=dict(artist='Me'), bitrate=1800)
        ani.save("movie2.mp4",writer=writer)

        print('tcost',t)
        
        plt.show()
        pass
    pass

def polt_dynamic_fig5_1():
    
    Jdeg, eefxyz= getData_from_txt(dir='C:\\Users\\Hades\\Desktop\\paper\\test_23-8-13\\Jdeg_from_Spoly_D7.txt')
    Jdeg = Jdeg*pi/180
    
    plot_from_data_dynamic(mode='Jdeg',indexToPlot=[0,1,2,3,4,5],t=2.466549246058226,data=Jdeg)
    pass

def polt_dynamic_fig5_2():
    
    xyz_EulerZYX = get_xyzEulerZYX(mode='xyzEulerZYX',dir='C:\\Users\\Hades\\Desktop\\paper\\code\\data_txt\\Jdeg_from_DVsync_poly_D1_7.txt')
    
    plot_from_data_dynamic(mode='pose',indexToPlot=[0,1,2],t=0.601,data=xyz_EulerZYX)
    
    pass



if __name__=="__main__":
    
    s_pose = np.array([0,    0,  90,  0,  10,   0])
    e_pose = np.array([30, -30,   0,  0,  100,  90]) 
    Dmax = np.array([0,0,0,0,0,0])
    Vmax = np.array([60,60,60,60,60,60]) 
    Amax = np.array([60,60,60,60,60,60]) 
    Jmax = np.array([300,300,300,300,300,300])
    Smax = np.array([500,500,500,500,500,500]) 
    Jdeg,Tcost = quartic_TimePoly(s_pose,e_pose,Dmax,Vmax,Amax,Jmax,Smax,poly_num=1000)
    plot_from_data(mode='Jdeg',indexToPlot=[0,1,2,3,4,5],t=Tcost,data=Jdeg, defualt_xticks=1)
    # polt_dynamic_fig5_2()
    # plot_fig5_1()

    # tcost=time_poly()
    # DV_poly()
    # xyz_EulerZYX = get_xyzEulerZYX(mode='xyzEulerZYX',dir='C:\\Users\\Hades\\Desktop\\paper\\code\\data_txt\\Jdeg_from_DVsync_poly_D1_7.txt')
    # plot_from_data(mode='pose',indexToPlot=[0,1,2],t=0.601,data=xyz_EulerZYX)
    
    # Jdeg, eefxyz= getData_from_txt(dir='C:\WorkFile\MatlabWork\Ti5\matlabsim\Jdeg_from_DVsync_poly_D1_7.txt')
    # save_xyzEuleZYX2txt(mode='Jdeg',param=Jdeg)
    # sync_tcost   D1=10 t_cost 5.666666666666668   D1=7  t_cost 5.166666666666666   t=tlist[0]
    
    # Jdeg, eefxyz= getData_from_txt(dir='C:\\Users\\Hades\\Desktop\\paper\\test_23-8-13\\Jdeg_from_Spoly_D7.txt')
    # Jdeg = Jdeg*pi/180
    # plot_from_data(mode='Jdeg',indexToPlot=[0,1,2,3,4,5],t=2.466549246058226,data=Jdeg)
    
    # d_list,tcost = test4()
    # d_list = d_list*pi/180
    # plot_from_data(mode='Jdeg',indexToPlot=[0,1],t=5.166666666666666,data=Jdeg)
   
    # d_list,tcost = test4_sameAcc()
    # d_list = d_list*pi/180
    # plot_from_data(mode='Jdeg',indexToPlot=[0,1],t=tcost[0],data=d_list)
    
    
    
    
    
    pass


